import numpy as np
import math

class trajectory(object):
    def __init__(self, x, y, yaw):
        self.x = x
        self.y = y
        self.yaw = yaw

def TestLinearTrajectory(start_x, end_x, y):
    # 直线轨迹
    cx = np.linspace(start_x, end_x, 2000, endpoint=False)
    cx = cx.tolist()
    cy = [y] * len(cx)
    cyaw = [0] * len(cx)
    
    # 定义
    traj = trajectory(cx, cy, cyaw)
    cx = traj.x
    cy = traj.y
    cyaw = traj.yaw
    
    return traj, cx, cy, cyaw

# TAOMIN
def TestTwoLinearTrajectory(start_x, end_x, y, angle_): #20221114 TaoMin
    
    L = 0.28 # 看车道示意图得到L
    
    # 斜线轨迹
    cx1 = np.linspace(start_x, -L, 1000, endpoint = False)        
    cx1 = cx1.tolist()
    cy1 = np.linspace((start_x + L) * math.tan(angle_), y, 1000, endpoint = False)
    cy1 = cy1.tolist()
    cyaw1 = [angle_] * len(cx1)

    # 直线轨迹
    cx2 = np.linspace(-L, end_x, 1000, endpoint = False)
    cx2 = cx2.tolist()
    cy2 = [y] * len(cx2)
    cyaw2 = [0] * len(cx2)
    
    # 轨迹加和
    cx = cx1 + cx2
    cy = cy1 + cy2
    cyaw = cyaw1 + cyaw2
    
    # 定义
    traj = trajectory(cx, cy, cyaw)
    cx = traj.x
    cy = traj.y
    cyaw = traj.yaw
    
    return traj, cx, cy, cyaw

def TestDiagonalLinearTrajectory(start_x, end_x, y, angle_): #20221115 TaoMin
    
    # 直线轨迹 0.786
    cx = np.linspace(start_x, end_x, 1000, endpoint = False)        
    cx = cx.tolist()
    cy = np.linspace(-1, y, 1000, endpoint = False)
    cy = cy.tolist()
    cyaw = [angle_] * len(cx)
    
    # 定义
    traj = trajectory(cx, cy, cyaw)
    cx = traj.x
    cy = traj.y
    cyaw = traj.yaw
    
    return traj, cx, cy, cyaw

def TestCircleTrajectory(r, n): #202211120 TaoMin
    
    # 圆形轨迹
    angle = np.linspace(np.pi/2, np.pi/2 + np.pi*2, 3600, endpoint=False)
    
    R = [r] * len(angle)
    func = lambda x,y : x*y
    cos_angle = np.cos(angle)
    sin_angle = np.sin(angle)
    cx = map(func, R, cos_angle)
    cy = map(func, R, sin_angle)
    cx = list(cx)
    cy = list(cy)
    
    cyaw = [angle[i] - 3*np.pi/2 for i in range(len(angle))]
    # cyaw = cyaw.tolist()
    
    # 定义
    traj = trajectory(cx, cy, cyaw)
    cx = traj.x
    cy = traj.y
    cyaw = traj.yaw
    
    return traj, cx, cy, cyaw

# def TestAnyCircleTrajectory(r, msg): #202211121 TaoMin
    
#     # 任意位置圆形轨迹 只针对2车
#     x_source = msg.robot_2.pos.x
#     y_source = msg.robot_2.pos.y
#     yaw_source = msg.robot_2.angle
#     xa_ = r * math.sin(yaw_source)
#     ya_ = r * math.cos(yaw_source)
#     x_ = x_source - xa_
#     y_ = y_source - ya_ #相对坐标系原点坐标（圆心坐标） 以小车在第四象限起始为例
    
#     # ------------- 以绝对坐标系原点为圆心的一个原轨迹 -------------#
#     angle = np.linspace(np.pi/2, np.pi/2 + np.pi*2, 3600, endpoint=False)
#     R = [r] * len(angle)
#     func = lambda x,y : x*y
#     cos_angle = np.cos(angle)
#     sin_angle = np.sin(angle)
#     cx = map(func, R, cos_angle)
#     cy = map(func, R, sin_angle)
#     cx = list(cx)
#     cy = list(cy)
#     cyaw = [angle[i] - 3*np.pi/2 for i in range(len(angle))]
#     # ------------- 以绝对坐标系原点为圆心的一个原轨迹 -------------#
    
#     # ---------偏移cx、cy---------#
#     cx_truth = [x_ + ]
    
#     # 定义
#     traj = trajectory(cx, cy, cyaw)
#     cx = traj.x
#     cy = traj.y
#     cyaw = traj.yaw
    
#     return traj, cx, cy, cyaw